/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "virtuallinebias.h"
#include <qdebug.h>
#include <QSettings>
#include <QVariant>
#include "filteringalgorithm.h"

#define D_FORWARD_R_FILTER_COUNT 6

#define D_PRINT_VIRTUAL_MOVING_DEBUGTTT1 0//无关
#define D_PRINT_VIRTUAL_MOVING_DEBUGTTT2 0//无关
#define D_PRINT_VIRTUAL_MOVING_DEBUGTTT3 0//无关
#define D_PRINT_VIRTUAL_MOVING_DEBUGTTT4 0//无关
#define D_PRINT_VIRTUAL_MOVING_DEBUGTTT5 0//无关
#define D_PRINT_VIRTUAL_MOVING_DEBUGTTT6 0//无关
#define D_PRINT_VIRTUAL_MOVING_DEBUGTTT7 0//无关
#define D_PRINT_VIRTUAL_MOVING_DEBUGTTT8 0//无关

VirtualLineBias::VirtualLineBias(QString configFileIn, double deltaTimeIn)
{
    configFile=configFileIn;
    deltaTime=deltaTimeIn;
    deltaDistance_x=0;
    deltaDistance_y=0;
    deltaAngle=0;
    forward_x=0;
    forward_y=0;
    forward_r=0;
    absoluteDistance=0;
    absoluteAngle=0;
    calculateBiasType=E_BIAS_TYPE_NONE;
    lastBiasType=E_BIAS_TYPE_NONE;
    lastBiasType_old=E_BIAS_TYPE_NONE;
    kp_errorDistance_x=0.2;
    kp_errorDistance_y=0.2;
    kp_angleDistance=0.2;
    kp_errorDistance_x_vmark=0.2;
    kp_errorDistance_y_vmark=0.2;
    kp_angleDistance_vmark=0.2;
    isInitialOkStatus=false;
    isEnableKpChange=true;
    debugFlag=0;
    pathControlStrategy= E_PATH_CONTROL_STRATEGY_DIFFERENTIAL;
//    filtering_feedforwardR==new FilteringAlgorithm;//两个等于号不报错。
    filtering_feedforwardX=new FilteringAlgorithm;//两个等于号不报错。
    filtering_feedforwardY=new FilteringAlgorithm;//两个等于号不报错。
    filtering_feedforwardR=new FilteringAlgorithm;//两个等于号不报错。
    filtering_moveR=new FilteringAlgorithm;
    filtering_moveY=new FilteringAlgorithm;
    filtering_moveX=new FilteringAlgorithm;

    vmark_filterCount=14;
    common_filterCount=6;
    if(1==initialConfigFile(configFileIn))
    {
        isInitialOkStatus=true;
    }
}

double VirtualLineBias::getSteeringMaxSpeed()
{
    return steeringMaxSpeed;
}

double VirtualLineBias::getSteeringMaxAcceration()
{
    return steeringMaxAcceration;
}



int VirtualLineBias::setDebugFlag(int flagIn)
{
    debugFlag=flagIn;
    return 1;
}

int VirtualLineBias::setMaccrumIdealMovingLineAngle(double idealAngleIn)
{
    idealAngle_vehicle=idealAngleIn;
    return 1;
}

int VirtualLineBias::setAgvPathFollowStrategy(E_PATH_CONTROL_STRATEGY stragetyIn)
{
    pathControlStrategy=stragetyIn;
    return 1;
}

int VirtualLineBias::setVirtualLineEnableKpChange(bool isEnable)
{
    qDebug()<<"setVirtualLineEnableKpChange"<<isEnable<<"kp_errorDistance_new"<<kp_errorDistance_x_new
              <<"kp_angleDistance_new"<<kp_angleDistance_new;
    isEnableKpChange=isEnable;
    return 1;
}

bool VirtualLineBias::isInitialOk()
{
    return isInitialOkStatus;
}

int VirtualLineBias::setVirtualLinePid(double kpDistanceErrorIn, double kpAngleDistanceIn,
                                       double kpOnlyRotationIn, double kpDistanceError_yIn)
{
    qDebug()<<"VirtualLineBias::setKp_errorDistance,before kp_errorDistance="<<kp_errorDistance_x
           <<" kp_angleDistance"<<kp_angleDistance<<" kp_onlyRotation"<<kp_onlyRotation
             <<" kp_errorDistance_y"<<kp_errorDistance_y;
    kp_errorDistance_x=kpDistanceErrorIn;
    kp_errorDistance_y=kpDistanceError_yIn;
    kp_angleDistance=kpAngleDistanceIn;
    kp_onlyRotation=kpOnlyRotationIn;
    qDebug()<<"VirtualLineBias::setKp_errorDistance,after kp_errorDistance="<<kp_errorDistance_x
              <<" kp_angleDistance"<<kp_angleDistance<<" kp_onlyRotation"<<kp_onlyRotation
                <<" kp_errorDistance_y"<<kp_errorDistance_y;
    return 1;
}

E_BIAS_TYPE VirtualLineBias::getCalculateBiasType()
{
    return calculateBiasType;
}

int VirtualLineBias::setCalculateBiasType(E_BIAS_TYPE calculateBiasTypeIn)
{
    calculateBiasType=calculateBiasTypeIn;
    calculateBiasType_old_forDebug=calculateBiasTypeIn;
    return 1;
}

int VirtualLineBias::setCalculateBiasType_noDebug(E_BIAS_TYPE calculateBiasTypeIn)
{
    calculateBiasType=calculateBiasTypeIn;
    if(2==debugFlag)
    {
        qDebug()<<"calculateBiasType_old_forDebug"<<calculateBiasType_old_forDebug;
    }

    return 1;
}

int VirtualLineBias::setPositionFeedback(VehiclePosition vehiclePositionIn,VehicleVelocity currentVehicleVelocityIn)
{
    currentVehiclePosition_old=currentVehiclePosition;
    currentVehicleVelocity_feedback=currentVehicleVelocityIn;
    currentVehiclePosition=vehiclePositionIn;
    return 1;
}

int VirtualLineBias::setVehicleVelCommand(VehicleVelocity currentVehicleVelocityIn)
{
    currentVehicleVelocity_command=currentVehicleVelocityIn;
    return 1;
}

int VirtualLineBias::getBias(BiasControlInfo &controlInfoOut)
{
    controlInfoOut.forward_x=forward_x;
    controlInfoOut.forward_y=forward_y;
    controlInfoOut.forward_r=forward_r;
    controlInfoOut.deltaDistanceX=deltaDistance_x;
    controlInfoOut.deltaDistanceY=deltaDistance_y;
    controlInfoOut.deltaAngle=deltaAngle;
    controlInfoOut.absoluteDistance=absoluteDistance;
    controlInfoOut.absoluteAngle=absoluteAngle;
    controlInfoOut.isPositiveDirection=isPositiveDirection;
    controlInfoOut.isPidStop=isPidStop;
    controlInfoOut.distanceFollowError=distanceFollowError;
    controlInfoOut.angleFollowError=angleFollowError;
    controlInfoOut.calculateBiasType=calculateBiasType;
    return 1;
}

int VirtualLineBias::setVehicleDirection(bool isPositiveIn)
{
    isPositiveDirection=isPositiveIn;
    return 1;
}

int VirtualLineBias::setPidStop(bool isStopIn)
{
    isPidStop=isStopIn;

    return 1;
}


int VirtualLineBias::stopOutput()
{
    if(2==debugFlag)
    {
        qDebug()<<"VirtualLineBias::stopOutput()";
    }
    forward_x=0;
    forward_y=0;
    forward_r=0;
    deltaDistance_x=0;
    deltaDistance_y=0;
    deltaAngle=0;
//    absoluteDistance=0;
//    absoluteAngle=0;
    isPidStop=true;
    filtering_moveR->resetAll();
    filtering_moveY->resetAll();
    filtering_moveX->resetAll();
    filtering_feedforwardX->resetAll();
    filtering_feedforwardY->resetAll();
    filtering_feedforwardR->resetAll();
    lastBiasType=E_BIAS_TYPE_NONE;
    lastBiasType_old=E_BIAS_TYPE_NONE;
    return 1;
}

int VirtualLineBias::initialConfigFile(QString fileIn)
{

    QVariant strConfigValue;
    QString firstTimeFlagStr = fileIn;
    QString strConfigGroup = "";
    QString strConfigKey = "";

    QSettings settings(firstTimeFlagStr,QSettings::IniFormat);
    settings.setIniCodec("UTF-8");

    strConfigGroup = "VirtualLineBias";
    settings.beginGroup(strConfigGroup);

    //kp_errorDistance
    strConfigKey = "kp_errorDistance_x";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    kp_errorDistance_x= strConfigValue.toDouble();

    //kp_errorDistance_vmark
    strConfigKey = "kp_errorDistance_x_vmark";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    kp_errorDistance_x_vmark= strConfigValue.toDouble();

    //kp_errorDistance_y
    strConfigKey = "kp_errorDistance_y";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    kp_errorDistance_y= strConfigValue.toDouble();

    //kp_errorDistance_y_vmark
    strConfigKey = "kp_errorDistance_y_vmark";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    kp_errorDistance_y_vmark= strConfigValue.toDouble();

    //kp_onlyRotation
    strConfigKey = "kp_onlyRotation";
    if(false==settings.contains(strConfigKey))
    {
        return -2;
    }
    strConfigValue = settings.value(strConfigKey);
    kp_onlyRotation= strConfigValue.toDouble();

    //kp_onlyRotation_vmark
    strConfigKey = "kp_onlyRotation_vmark";
    if(false==settings.contains(strConfigKey))
    {
        return -2;
    }
    strConfigValue = settings.value(strConfigKey);
    kp_onlyRotation_vmark= strConfigValue.toDouble();

    //kp_angleDistance
    strConfigKey = "kp_angleDistance";
    if(false==settings.contains(strConfigKey))
    {
        return -2;
    }
    strConfigValue = settings.value(strConfigKey);
    kp_angleDistance= strConfigValue.toDouble();

    //kp_angleDistance_vmark
    strConfigKey = "kp_angleDistance_vmark";
    if(false==settings.contains(strConfigKey))
    {
        return -2;
    }
    strConfigValue = settings.value(strConfigKey);
    kp_angleDistance_vmark= strConfigValue.toDouble();

    //kp_start_velocity
    strConfigKey = "kp_start_velocity";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    kp_start_velocity= strConfigValue.toDouble();

    //kp_end_velocity
    strConfigKey = "kp_end_velocity";
    if(false==settings.contains(strConfigKey))
    {
        return -2;
    }
    strConfigValue = settings.value(strConfigKey);
    kp_end_velocity= strConfigValue.toDouble();

    //kp_end_ratio
    strConfigKey = "kp_end_ratio";
    if(false==settings.contains(strConfigKey))
    {
        return -2;
    }
    strConfigValue = settings.value(strConfigKey);
    kp_end_ratio= strConfigValue.toDouble();

    //kp_start_ratio
    strConfigKey = "kp_start_ratio";
    if(false==settings.contains(strConfigKey))
    {
        return -2;
    }
    strConfigValue = settings.value(strConfigKey);
    kp_start_ratio= strConfigValue.toDouble();

    settings.endGroup();
    //--------------------------------------------------


    strConfigGroup = "MechanicalInfo";
    settings.beginGroup(strConfigGroup);

    //vehicletype
    strConfigKey = "vehicletype";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    vehicletype= (E_VEHICLE_TYPE)strConfigValue.toInt();

    //steeringMaxSpeed
    strConfigKey = "steeringMaxSpeed";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    steeringMaxSpeed= strConfigValue.toDouble();


    //steeringMaxAcceration
    strConfigKey = "steeringMaxAcceration";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    steeringMaxAcceration= strConfigValue.toDouble();


    settings.endGroup();

    qDebug() << "VirtualLineBias::initialConfigFile sucess";
    return 1;
}


int VirtualLineBias::calculateMicroBias(VehiclePosition oldPos, VehiclePosition newPos, VehiclePosition actPos,
                                           double &deltaDistanceOut, double &deltaAngleOut)
{
    //not move to exit，　无法计算角度
    if(oldPos.x==newPos.x && oldPos.y==newPos.y && oldPos.rotate==newPos.rotate)
    {
        //不要计算，继续使用上次的偏差
        return -1;
    }



    double idealMovingAngle;
    double actMovingAngle;
    bool isPassOver=false;
    idealMovingAngle=atan2(newPos.y-oldPos.y,newPos.x-oldPos.x);//(-pi,pi)
    actMovingAngle=atan2(actPos.y-newPos.y,actPos.x-newPos.x);//(-pi,pi)
//    getVehicleMovingDirection(actMovingAngle);
    //direction
    double tmpDirection;
    if(true==isPositiveDirection)
    {
        tmpDirection=1;
    }
    else
    {
        tmpDirection=-1;
    }
    lastMoveDirection=idealMovingAngle;
    double totalDistance=sqrt((actPos.x-newPos.x)*(actPos.x-newPos.x));
    double movingAngleBias=transformToPi(idealMovingAngle-actMovingAngle);//error
    qDebug()<<"------ lastMoveDirection=="<<lastMoveDirection;

    if(M_PI_2>=movingAngleBias && -M_PI_2<=movingAngleBias)
    {
        isPassOver=false;
        deltaAngleOut=0;
        deltaDistanceOut=totalDistance*tmpDirection;//需要判断位置超前还是滞后。

        if(D_PRINT_VIRTUAL_MOVING_DEBUGTTT1 )
        {
            qDebug()<<"11111-f tmpAngleBias="<<movingAngleBias
                   <<"actAngle"<<actMovingAngle<<"  deltaDistanceOut"<<deltaDistanceOut;
        }

    }
    else
    {
        isPassOver=true;
        deltaAngleOut=0;
        deltaDistanceOut=-totalDistance*tmpDirection;//需要判断位置超前还是滞后。
        if(D_PRINT_VIRTUAL_MOVING_DEBUGTTT2 )
        {
            qDebug()<<"111111-r tmpAngleBias="<<movingAngleBias<<"actAngle"<<actMovingAngle
                   <<"  deltaDistanceOut"<<deltaDistanceOut;
        }

    }
    if(true==isPositiveDirection && deltaDistanceOut>0)
    {
        deltaDistanceOut=0;
        if(D_PRINT_VIRTUAL_MOVING_DEBUGTTT3 )
        {
            qDebug()<<"not allow true==isPositiveDirection && deltaDistanceOut>0";
        }

    }
    else if(false==isPositiveDirection && deltaDistanceOut<0)
    {
        deltaDistanceOut=0;
        if(D_PRINT_VIRTUAL_MOVING_DEBUGTTT4 )
        {
            qDebug()<<"not allow false==isPositiveDirection && deltaDistanceOut<0";
        }

    }





    return 1;
}

int VirtualLineBias::calculateMicroBias_rotation(double deltaTimeIn,
                         VehiclePosition oldPos_command, VehiclePosition newPos_command,
                         VehiclePosition pos_feedback, VehiclePosition pos_feedback_old,
                         double &distanceFollowErrorOut,double &angleFollowErrorOut,
                         double &deltaAngleOut, double &rotateFeedwardOut,
                         double &movingAngleBiasOut,E_BIAS_TYPE lastBiasTypeIn)
{
    if(2==debugFlag)
    {
        qDebug()<<"VirtualLineBias::calculateMicroBias_rotation begin 111";
    }
    static double deltaAngle_old=0;
    double tmpAngle=0;
    double limitAngleBias=M_PI*35.0/180.0;//限制输出幅度20181119
    tmpAngle=-angleRotationJudgement(newPos_command.rotate,pos_feedback.rotate);//用了负号，华磊
    rotateFeedwardOut=angleRotationJudgement(newPos_command.rotate,oldPos_command.rotate)/deltaTimeIn;//负号不对
    //跟随误差计算
    angleFollowErrorOut=angleRotationJudgement(newPos_command.rotate,pos_feedback.rotate);
    distanceFollowErrorOut=sqrt( (pos_feedback.x-newPos_command.x)*(pos_feedback.x-newPos_command.x)+
                        (pos_feedback.y-newPos_command.y)*(pos_feedback.y-newPos_command.y));

    movingAngleBiasOut=tmpAngle;
    tmpAngle=kp_onlyRotation_new*tmpAngle;
    if(tmpAngle>limitAngleBias)
    {
        tmpAngle=limitAngleBias;
//        qDebug()<<"calculateMicroBias_rotation ppp tmpAngle"<<tmpAngle<<"limitAngleBias"<<limitAngleBias;
    }
    else if(tmpAngle<-limitAngleBias)
    {
        tmpAngle=-limitAngleBias;
//        qDebug()<<"calculateMicroBias_rotation nnn tmpAngle"<<tmpAngle<<"limitAngleBias"<<limitAngleBias;
    }
    deltaAngleOut=tmpAngle;
    //第一个点角度偏差计算有异常。因为直线计算的命令角度是不对的。偶尔会出现一个异常大点
    if(E_BIAS_TYPE_ROTATE!=lastBiasTypeIn || fabs(deltaAngle_old-deltaAngleOut)>0.35)
    {
        deltaAngleOut=0;
        rotateFeedwardOut=0;
        angleFollowErrorOut=0;
        distanceFollowErrorOut=0;
//        qDebug()<<"E_BIAS_TYPE_ROTATE==lastBiasTypeIn";
    }
    deltaAngle_old=deltaAngleOut;


    if(2==debugFlag)
    {
        qDebug()<<"deltaAngleOut="<<deltaAngleOut;
    }
//    qDebug()<<"22223 rotateFeedwardOut"<<rotateFeedwardOut;
    return 1;

}

double VirtualLineBias::angleRotationJudgement(double targetAngle, double initialAngle)
{
    double angle1,angle2,angle3;
    angle1=targetAngle-initialAngle;
    angle2=angle1+2*M_PI;
    angle3=angle1-2*M_PI;
    double minNum;
    if(fabs(angle1)<=fabs(angle2))
    {
        minNum=angle1;
    }
    else
    {
        minNum=angle2;
    }

    if(fabs(angle3)<=fabs(minNum))
    {
        minNum=angle3;
    }

    return minNum;

}
#define D_LINE_START_SEGMENT_LENGTH 0.05
#define D_KP_LOW_VEL 0.02
//http://www.doc88.com/p-6357459377381.html PAGE43
//stanley轨迹跟踪方法。不仅考虑角度偏移，也要考虑偏离轨道的距离和速度的比值。
int VirtualLineBias::calculateMicroBias_stanley(double deltaTimeIn, VehiclePosition oldPos_command, VehiclePosition newPos_command,
                                                VehiclePosition pos_feedback, VehiclePosition pos_feedback_old,
                                                double &distanceFollowErrorOut, double &angleFollowErrorOut,
                                                double &deltaDistanceOut, double &deltaAngleOut,
                                                double &forward_xOut, double &forward_rOut,
                                                double &movingAngleBiasOut, E_BIAS_TYPE lastBiasTypeIn)
{

    if(2==debugFlag)
    {
        qDebug()<<"VirtualLineBias::calculateMicroBias_stanley begin 111";
    }
    static double deltaAngleOut_old=0;
    static int stepIndex=0;
    static VehiclePosition startPosition;
    if(E_BIAS_TYPE_MOVE_LINE!=lastBiasTypeIn )
    {
        stepIndex=0;

        startPosition=newPos_command;
        if(D_PRINT_VIRTUAL_MOVING_DEBUGTTT5 )
        {
            qDebug()<<"debug5:111E_BIAS_TYPE_MOVE_LINE!=lastBiasTypeIn"<<lastBiasTypeIn;
            qDebug()<<"******startPosition"<<startPosition.x<<startPosition.y<<startPosition.rotate;
        }


    }
//    if(4==stepIndex)
//    {
//        startPosition=newPos_command;
//        qDebug()<<"******startPosition"<<startPosition.x<<startPosition.y<<startPosition.rotate;
//    }


//    static int firstCount=0;
    //not move to exit，　无法计算角度
    if(oldPos_command.x==newPos_command.x &&
            oldPos_command.y==newPos_command.y )// 不用判断rotate，　&& oldPos_command.rotate==newPos_command.rotate
    {
        if(2==debugFlag)
        {
            qDebug()<<"warnning,,,   oldPos_command.x==newPos_command.x";
        }
        //不要计算，继续使用上次的偏差
        return 0;
    }


    stepIndex++;

    //旋转后的一段距离不允许回退.
    bool isEnableBackPidOutPut=true;
    double distance_moved=sqrt( (startPosition.x-newPos_command.x)*(startPosition.x-newPos_command.x)+
                                (startPosition.y-newPos_command.y)*(startPosition.y-newPos_command.y));

    if(distance_moved<D_LINE_START_SEGMENT_LENGTH)
    {
        isEnableBackPidOutPut=false;
    }

    //direction
    double tmpDirection;
    if(true==isPositiveDirection)
    {
        tmpDirection=1;
    }
    else
    {
        tmpDirection=-1;
    }

    double idealMovingAngle;//轨迹理论角度
    double microDistanceAngle;//车体与轨迹偏心的衡量角度
    bool isPositiveDrive=false;//车头方向与路径运动方向是否一致。与isPositiveDirection比较判断异常情况

    //idealMovingAngle 精度不足时，就返回
    static int accuracyErrorCount=0;
    if(fabs(newPos_command.x-oldPos_command.x)<0.000001
            && fabs(newPos_command.y-oldPos_command.y)<0.000001
            && fabs(newPos_command.rotate-oldPos_command.rotate)<0.000001)
    {
        accuracyErrorCount++;
        if(2==debugFlag)
        {
            qDebug()<<"warnning,,,,,,,,idealMovingAngle 精度不足，输出归零";//设置0.0001时，2%的速度会达到这个条件。
        }
        return -1;
    }
    else
    {
        accuracyErrorCount=0;
    }
    if(accuracyErrorCount>100)
    {
        return -103;
    }

    idealMovingAngle=atan2(newPos_command.y-oldPos_command.y,newPos_command.x-oldPos_command.x);//(-pi,pi)
    microDistanceAngle=atan2(pos_feedback.y-newPos_command.y,pos_feedback.x-newPos_command.x);//(-pi,pi)
    double angleBias_errorDistance_idealLine=angleRotationJudgement(idealMovingAngle,microDistanceAngle);//开始对

    if(1!=checkStartMoveOrientation(isPositiveDirection,pos_feedback.rotate ,idealMovingAngle))
    {
        qDebug()<<"车头初始方向错误， error,,,,,,,,,,, 1!=checkStartMoveOrientation()";
        return -100;
    }

    //不对，直线规划没有使用.rotate
    forward_rOut=angleRotationJudgement(idealMovingAngle,lastMoveDirection)/deltaTimeIn;//负号不对，是否对？正号不确定是否对?
    if(1<forward_rOut || -1>forward_rOut )//不旋转，直接走直线会有第２～５位置的异常值|| stepIndex<6
    {
        if(2==debugFlag)
        {
            qDebug()<<"角度前馈过大，角度前馈归零 forward_rOut"<<forward_rOut<<"stepIndex"<<stepIndex<<"idealMovingAngle"
                   <<idealMovingAngle<<"lastMoveDirection"<<lastMoveDirection;
        }
        forward_rOut=0;//首次运行，第４次计算时，出现-3.1415926 异常数据。

    }
    double tmpVel=sqrt( (oldPos_command.x-newPos_command.x)*(oldPos_command.x-newPos_command.x)+
                       (oldPos_command.y-newPos_command.y)*(oldPos_command.y-newPos_command.y))/deltaTimeIn;//近似计算。正负号是否对？

    lastMoveDirection=idealMovingAngle;
//    double movingAngleBias=transformToPi(idealMovingAngle-microDistanceAngle);
  ////  double angleBias_errorDistance_idealLine=angleRotationJudgement(idealMovingAngle,microDistanceAngle);

    double totalDistance=sqrt((pos_feedback.y-newPos_command.y)*(pos_feedback.y-newPos_command.y)+
                              (pos_feedback.x-newPos_command.x)*(pos_feedback.x-newPos_command.x));

    double tmpVehicleMovingAngle;//车的命令运动方向
    double  angleBias_transVel_idealLine;//开始不对
    if(1==getVehicleMovingDirection(tmpVehicleMovingAngle))
    {
        //    angleOffSetForRotate=transformToPi(idealMovingAngle-tmpVehicleMovingAngle);
            angleBias_transVel_idealLine=angleRotationJudgement(idealMovingAngle,tmpVehicleMovingAngle);
            //防止偶尔掉头的时候不对,变换太大了，就不进行调整。
            if(angleBias_transVel_idealLine<-M_PI_2 || angleBias_transVel_idealLine> M_PI_2)
            {
                if(2==debugFlag)
                {
                    qDebug()<<"速度方向不对，输出归零  angleBias_transVel_idealLine<-M_PI_2 || angleBias_transVel_idealLine> M_PI_2"
                           <<angleBias_transVel_idealLine;
                }
                angleBias_transVel_idealLine=0;
                totalDistance= 0;//后加
        //        forward_rOut=0;
        //        forward_xOut=0;
            }
    }
    else
    {
        if(2==debugFlag)
        {
            qDebug()<<"零速度启动，无速度方向 getVehicleMovingDirection(tmpVehicleMovingAngle)"
                   <<getVehicleMovingDirection(tmpVehicleMovingAngle);
        }

        angleBias_transVel_idealLine=0;
//        totalDistance= 0;//需要置０，否则会导致角度偏差计算错误，低速时会旋转很大的角度。不得置０，否则会不启动．
        //零速度启动，不对角度进行控制，只控制前进．
    }

    //使用前馈控制，超前了也需要调整，偏差为负。
    if(M_PI_2>=angleBias_errorDistance_idealLine && -M_PI_2<=angleBias_errorDistance_idealLine)//要求方向与当前运动方向相反
    {
//        if(true==isPositiveDirection)
//        {
//            qDebug()<<"要求车体前进行驶，但是当前车体方向与路径方向大于９０度，请先旋转好角度。";
//            addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",8107,robotId,deltaAngleOut);
//            return -1;
//        }
           isPositiveDrive=true;
//           ed=0;//不调整//使用前馈控制，超前了也需要调整，偏差为负。
//           angleBias_errorDistance_idealLine=0;//不调整//使用前馈控制，超前了也需要调整，偏差为负。
//           forward_rOut=0;
//           forward_xOut=0;
    }
    else//要求方向与当前运动方向相同
    {
//        if(false==isPositiveDirection)
//        {
//            qDebug()<<"要求车体倒退行驶，但是当前车体方向与路径方向大于９０度，请先旋转好角度。";
//            addMsg(ENUM_MSG_ERROR,"VirtualLineControl","VirtualLineControl",8106,robotId,deltaAngleOut);
//            return -1;
//        }

        isPositiveDrive=false;

    }
    forward_xOut=tmpVel*tmpDirection;
    deltaDistanceOut=totalDistance*cos(angleBias_errorDistance_idealLine)*tmpDirection*kp_errorDistance_x_new;
    if(D_LINE_START_SEGMENT_LENGTH>distance_moved)
    {
        deltaDistanceOut=0.2*deltaDistanceOut;//防止旋转后加速冲击。
        //旋转后，反馈位置超前，５ｃｍ内没有输出，然后突然一个前馈输出，导致有点冲击声音。加速度越大越明显，伺服刚性越大越明显。
        if(2==debugFlag)
        {
            qDebug()<<"开始长度，距离输出减小，，，distance_moved"<<distance_moved;
        }
        if(D_KP_LOW_VEL>tmpVel)
        {
            deltaDistanceOut=0.6*deltaDistanceOut;//防止旋转后加速冲击。
            //旋转后，反馈位置超前，５ｃｍ内没有输出，然后突然一个前馈输出，导致有点冲击声音。加速度越大越明显，伺服刚性越大越明显。
            if(2==debugFlag)
            {
                qDebug()<<"开始长度，低速时距离输出减小，，，tmpVel"<<tmpVel;
            }
        }
    }


    double errorLineDistance=totalDistance*sin(angleBias_errorDistance_idealLine);

    //stanley公式.需要和速度关联，速度慢调整小，速度快，调整大
//    deltaAngleOut=-totalDistance*(angleBias_transVel_idealLine+kp_errorDistance*angleBias_errorDistance_idealLine);
   //和速度关联
//    deltaAngleOut=-totalDistance*(angleBias_transVel_idealLine+kp_errorDistance*errorLineDistance);
   //不和速度关联
   deltaAngleOut=-(kp_angleDistance_new*angleBias_transVel_idealLine+kp_errorDistance_y_new*errorLineDistance);

   movingAngleBiasOut=angleBias_errorDistance_idealLine;
   if(2==debugFlag)
   {
       qDebug()<<"关键变量****distance_moved"<<distance_moved<<" angleBias_transVel_idealLine"<<angleBias_transVel_idealLine
              <<" errorLineDistance"<<errorLineDistance<<" angleBias_errorDistance_idealLine"<<angleBias_errorDistance_idealLine
             <<" idealMovingAngle"<<idealMovingAngle<<" tmpVehicleMovingAngle"<<tmpVehicleMovingAngle;
   }
   //跟随误差计算
   angleFollowErrorOut=angleBias_transVel_idealLine;
   distanceFollowErrorOut=sqrt( (pos_feedback.x-newPos_command.x)*(pos_feedback.x-newPos_command.x)+
                       (pos_feedback.y-newPos_command.y)*(pos_feedback.y-newPos_command.y));

    //超前或滞后判断　－－－－－－－－ｘ方向如果过了，但是不要回退。//使用前馈控制，超前了也需要调整，偏差为负。
    if(cos(angleBias_errorDistance_idealLine)<0)//滞后
    {

        //        forward_xOut=tmpVel*tmpDirection;//bug,导致减速pid不变。
    }
    else//位置超前
    {
        //使用前馈控制，超前了也需要调整，偏差为负。
        //        forward_rOut=0;
        //        forward_xOut=0;//会导致卡

        if(false==isEnableBackPidOutPut)
        {
            if(2==debugFlag)
            {
                qDebug()<<"位置超前，输出归零 false==isEnableBackPidOutPut,cos(angleBias_errorDistance_idealLine)"
                       <<cos(angleBias_errorDistance_idealLine);
            }
            deltaAngleOut=0;
            deltaDistanceOut=0;
            forward_rOut=0;
//            forward_xOut=0;//会导致卡.修改２０２００５１７，不要为０，使前馈正常，否则开始会抖动．
        }
        if(2==debugFlag)
        {
            qDebug()<<"ｘ方向如果过了，但是不要回退。angleBias_errorDistance_idealLine"<<angleBias_errorDistance_idealLine;
        }

    }

    //高速度时会有明显摆动，所以要做限制处理。如果刚性调低，又会导致低速的时候调整能力太弱。
//    double limitAngleBias=M_PI*15.0/180.0;//限制输出幅度20181031
//    if(movingAngleBiasOut>limitAngleBias)
//    {
//        movingAngleBiasOut=limitAngleBias;
//    }
//    else if(movingAngleBiasOut<-limitAngleBias)
//    {
//        movingAngleBiasOut=-limitAngleBias;
//    }


//    qDebug()<<"stanley---- deltaDistanceOut="<<deltaDistanceOut<<", movingAngleBias="
//           <<movingAngleBias<<",ed="<<ed<<"deltaAngleOut="<<deltaAngleOut<<",angleOffSetForRotate="
//          <<angleOffSetForRotate;

    //错误点剔除－－－－－－－－－－－第一个点角度偏差计算有异常。偶尔出现异常点（需要剔除２个点）
    double tmpAngleBias=fabs(deltaAngleOut-deltaAngleOut_old);
    static int angleBiasErrorCount=0;
    if(E_BIAS_TYPE_MOVE_LINE!=lastBiasTypeIn || tmpAngleBias>0.35)
    {
        if(D_PRINT_VIRTUAL_MOVING_DEBUGTTT6 )
        {
            qDebug()<<"debug6: 不运动提示：lastBiasTypeIn="<<lastBiasTypeIn<<" tmpAngleBias="<<tmpAngleBias
                   <<" deltaAngleOut"<<deltaAngleOut<<" deltaAngleOut_old"<<deltaAngleOut_old;
        }


        if(2==debugFlag)
        {
            qDebug()<<"角度输出关闭：lastBiasTypeIn="<<lastBiasTypeIn<<" tmpAngleBias="<<tmpAngleBias;
        }
        deltaAngleOut=0;
//        deltaDistanceOut=0;//不能将位移归零，否则直线开始不动．
        forward_rOut=0;//必须要打开，否则会第一次错误输出。
        forward_xOut=0;//这个是正确的。
        angleFollowErrorOut=0;
//        distanceFollowErrorOut=0;//导致最后才报跟随误差过大
        stepIndex=0;//导致ＡＧＶ一直停止不动
//        qDebug()<<"E_BIAS_TYPE_ROTATE==lastBiasTypeIn";
        angleBiasErrorCount++;

        if(angleBiasErrorCount>50 && tmpAngleBias>0.35)
        {
            deltaAngleOut_old=deltaAngleOut;
            return -101;
        }
    }
    else
    {
        angleBiasErrorCount=0;
    }

    deltaAngleOut_old=deltaAngleOut;
    if(2==debugFlag)
    {
        qDebug()<<"末尾输出deltaAngleOut="<<deltaAngleOut<<" deltaDistanceOut="<<deltaDistanceOut;
    }
    return 1;
}

int VirtualLineBias::calculateMicroBias_maccrum(double deltaTimeIn, VehiclePosition oldPos_command, VehiclePosition newPos_command,
                                                VehiclePosition pos_feedback, VehiclePosition pos_feedback_old, double &distanceFollowErrorOut, double &angleFollowErrorOut,
                                        double &deltaDistanceOut_x, double &deltaDistanceOut_y, double &deltaAngleOut,
                                                double &forward_xOut, double &forward_yOut, double &forward_rOut,
                                                double &movingAngleBiasOut, E_BIAS_TYPE lastBiasTypeIn)
{
    if(2==debugFlag)
    {
        qDebug()<<"VirtualLineBias::calculateMicroBias_maccrum begin 111";
    }
    //没有机器可以测试，暂时屏蔽前馈。
    forward_xOut=0;
    forward_yOut=0;
    forward_rOut=0;
    //    static int firstCount=0;
        //not move to exit，　无法计算角度
        if(oldPos_command.x==newPos_command.x &&
                oldPos_command.y==newPos_command.y && oldPos_command.rotate==newPos_command.rotate)
        {
            //不要计算，继续使用上次的偏差
    //        deltaAngleOut=0;
    //        firstCount=0;
            return -1;
        }

        static double deltaAngleOut_old=0;


        double deltaForwardDistance;
        double idealMovingAngle;//轨迹理论角度
        double microDistanceAngle;//车体与轨迹偏心的衡量角度
        idealMovingAngle=atan2(newPos_command.y-oldPos_command.y,newPos_command.x-oldPos_command.x);//(-pi,pi)
        microDistanceAngle=atan2(pos_feedback.y-newPos_command.y,pos_feedback.x-newPos_command.x);//(-pi,pi)

        lastMoveDirection=idealMovingAngle;
    //    double movingAngleBias=transformToPi(idealMovingAngle-microDistanceAngle);
        double angleBias_errorDistance_idealLine=angleRotationJudgement(idealMovingAngle,microDistanceAngle);

        double totalLagDistance=sqrt((pos_feedback.y-newPos_command.y)*(pos_feedback.y-newPos_command.y)+
                                  (pos_feedback.x-newPos_command.x)*(pos_feedback.x-newPos_command.x));


        //要求方向与当前运动方向相反
        if(M_PI_2>=angleBias_errorDistance_idealLine && -M_PI_2<=angleBias_errorDistance_idealLine)
        {
               angleBias_errorDistance_idealLine=0;//不调整?????
        }
        else//要求方向与当前运动方向相同
        {

        }
        //ｘ方向如果过了，但是不要回退。
        if(cos(angleBias_errorDistance_idealLine)<0)//error
        {
            deltaForwardDistance=totalLagDistance*cos(angleBias_errorDistance_idealLine);
        }
        else
        {
            deltaForwardDistance=0;
        }
//        double deltaBesideDistance=totalLagDistance*sin(angleBias_errorDistance_idealLine);
        // 增益计算
        movingAngleBiasOut=angleBias_errorDistance_idealLine;

//        translateErrorDistanceToVehicleDistance(pos_feedback.rotate,idealMovingAngle,
//                                                deltaForwardDistance*kp_errorDistance_new,
//                                                deltaBesideDistance*kp_errorDistance_y_new,
//                                                deltaDistanceOut_x,deltaDistanceOut_y);
        translateErrorDistanceToVehicleDistance_new(pos_feedback.rotate,idealMovingAngle,
                                                    angleBias_errorDistance_idealLine,totalLagDistance,
                                             kp_errorDistance_x_new,deltaDistanceOut_x,deltaDistanceOut_y);
        double tmp_angleOffset=-angleRotationJudgement(idealAngle_vehicle,pos_feedback.rotate);//用了负号，华磊
        deltaAngleOut=tmp_angleOffset*kp_angleDistance_new;

        //跟随误差计算
        angleFollowErrorOut=tmp_angleOffset;
        distanceFollowErrorOut=sqrt( (pos_feedback.x-newPos_command.x)*(pos_feedback.x-newPos_command.x)+
                            (pos_feedback.y-newPos_command.y)*(pos_feedback.y-newPos_command.y));

        //第一个点角度偏差计算有异常。偶尔出现异常点
        if(E_BIAS_TYPE_MOVE_LINE!=lastBiasTypeIn || fabs(deltaAngleOut-deltaAngleOut_old)>0.35)
        {
            deltaAngleOut=0;
            deltaDistanceOut_x=0;
            deltaDistanceOut_y=0;
            angleFollowErrorOut=0;
            distanceFollowErrorOut=0;
    //        qDebug()<<"E_BIAS_TYPE_ROTATE==lastBiasTypeIn";
        }
        deltaAngleOut_old=deltaAngleOut;
        if(2==debugFlag)
        {
            qDebug()<<"deltaAngleOut="<<deltaAngleOut<<" deltaDistanceOut_x="<<deltaDistanceOut_x
                      <<" deltaDistanceOut_y="<<deltaDistanceOut_y;
        }
        return 1;

}

#define D_REASONBABLE_VEL_FOR_TAN2 0.0001
int VirtualLineBias::getVehicleMovingDirection(double &directionOut)//实际传递进来的是手动模块自动命令速度，而不是反馈的速度。
{
    //容易出现０００，壁障减速后，抖动
//    if(fabs(currentVehiclePosition_old.x-currentVehiclePosition.x)<0.0000001
//            && fabs(currentVehiclePosition_old.y-currentVehiclePosition.y)<0.0000001
//            && fabs(currentVehiclePosition_old.rotate-currentVehiclePosition.rotate)<0.0000001)//0.0001低速时会抖动
//    {
//        qDebug()<<"getVehicleMovingDirection failed"<<currentVehiclePosition_old.x-currentVehiclePosition.x<<currentVehiclePosition_old.y-currentVehiclePosition.y
//               <<currentVehiclePosition_old.rotate-currentVehiclePosition.rotate;
//        return -1;
//    }
    //矛盾，用命令速度效果更加好。
    //计算车体合成平移速度夹角，相对于车体Ｘ轴。有个精度问题，如果ｘ和y方向的速度都接近０，则这个结果可能完全错误的。
    double tmpAngle;
    if(fabs(currentVehicleVelocity_command.y_move)>D_REASONBABLE_VEL_FOR_TAN2 || fabs(currentVehicleVelocity_command.x_move)>D_REASONBABLE_VEL_FOR_TAN2)
    {
        tmpAngle=atan2(currentVehicleVelocity_command.y_move,currentVehicleVelocity_command.x_move);

    }
    else
    {
        tmpAngle=0;
        //这种情况说明车是处于静止状态，没有方向．
        return -1;
    }


    //叠加车体的位置
    directionOut=tmpAngle+currentVehiclePosition.rotate;

    transformToPi(directionOut);
//    qDebug()<<"currentVehicleVelocity_command.y_move"<<currentVehicleVelocity_command.y_move
//              <<"currentVehicleVelocity_command.x_move"<<currentVehicleVelocity_command.x_move
//             <<"currentVehiclePosition.rotate"<<currentVehiclePosition.rotate<<"directionOut"<<directionOut;

    return 1;
}

int VirtualLineBias::calculateNewKp(double old_ea, double old_ed_x, double old_ed_y,   VehicleVelocity velocityIn,
                                    double &new_ea, double &new_ed_x, double &new_ed_y)
{

    //不考虑旋转
    double composedVel=sqrt(velocityIn.x_move*velocityIn.x_move+velocityIn.y_move*velocityIn.y_move);
    if(composedVel<kp_start_velocity)
    {
        new_ea=old_ea;
        new_ed_x=old_ed_x;
        new_ed_y=old_ed_y;
        return 1;
    }
    else if(composedVel>= kp_start_velocity && composedVel<kp_end_velocity)
    {
        double adjustRatio=kp_start_ratio+(composedVel-kp_start_velocity)*(kp_end_ratio-kp_start_ratio)/(kp_end_velocity-kp_start_velocity);
        new_ea=old_ea*adjustRatio;
        new_ed_x=old_ed_x*adjustRatio;
        new_ed_y=old_ed_y*adjustRatio;
        return 1;
    }
    else
    {
        new_ea=old_ea*kp_end_ratio;
        new_ed_x=old_ed_x*kp_end_ratio;
        new_ed_y=old_ed_y*kp_end_ratio;
        return 1;
    }
    return 1;

}

int VirtualLineBias::translateErrorDistanceToVehicleDistance(double vehicleAngle,double movingAngle,
                                                             double forwardErrorIn, double basideErrorIn,
                                                             double &xErrorOut, double &yErrorOut)
{
    //不对，basideErrorIn分解不对。
    double angleBias=angleRotationJudgement(movingAngle,vehicleAngle);
    xErrorOut=forwardErrorIn*cos(angleBias)+basideErrorIn*sin(angleBias);//?±对吗
    yErrorOut=forwardErrorIn*sin(angleBias)+basideErrorIn*cos(angleBias);//?±对吗
    return 1;
}

int VirtualLineBias::translateErrorDistanceToVehicleDistance_new(double vehicleAngle, double movingAngle,
                                                       double lagAngle, double lagDistance,double kpRatio,
                                                                 double &xErrorOut, double &yErrorOut)
{
    double angleBias=angleRotationJudgement(movingAngle,vehicleAngle);
//    double crossAngle=M_PI-angleBias-lagAngle;
    double crossAngle=lagAngle-angleBias;
    //1)+ + 左右和前进不对。２）－　－　左右不对。３）－　＋　前进不对。４）＋　－
    xErrorOut=kpRatio*lagDistance*cos(crossAngle);//?±对吗
    yErrorOut=-kpRatio*lagDistance*sin(crossAngle);//?±对吗
    return 1;
}

int VirtualLineBias::renewPidPara()
{
    //根据速度调整增益
    if(false==isEnableKpChange)
    {
        kp_angleDistance_new=kp_angleDistance_vmark;
        kp_errorDistance_x_new=kp_errorDistance_x_vmark;
        kp_errorDistance_y_new=kp_errorDistance_y_vmark;
        kp_onlyRotation_new=kp_onlyRotation_vmark;
    }
    else
    {
        kp_onlyRotation_new=kp_onlyRotation;
        calculateNewKp(kp_angleDistance,kp_errorDistance_x,kp_errorDistance_y,currentVehicleVelocity_command,
                       kp_angleDistance_new,kp_errorDistance_x_new,kp_errorDistance_y_new);
        //ｖ槽远处加速段，由于ｙ误差大，导致角度偏差计算出现非常大的误差。好像不能加
//        if(fabs(currentVehicleVelocity_command.x_move)<0.003)
//        {
//            kp_angleDistance_new=0.01;
//        }
//        else if(fabs(currentVehicleVelocity_command.x_move)<0.006)
//        {
//            kp_angleDistance_new=0.05;
//        }
//        else if(fabs(currentVehicleVelocity_command.x_move)<0.012)
//        {
//            kp_angleDistance_new=0.1;
//        }
//        else if(fabs(currentVehicleVelocity_command.x_move)<0.015)
//        {
//            kp_angleDistance_new=0.2;
//        }

    }
}

int VirtualLineBias::checkPositionFeedbackForBias(VehiclePosition posFeedback, VehicleVelocity vehicleVelocity, double &deltaDistanceOut_x,
                                                  double &deltaDistanceOut_y, double &deltaAngleOut)
{
    //还没有设定超过多少次反馈没变则报错。
    //使用这个函数的原因时，ros获取tf时候，不知道什么原因导致几个到几十个周期反馈值没有变换，导致偏差输出剧增。
    //连续２次没变，则偏差输出不变。
    //不允许超过10次，否则激光掉线后，tf位置不变，ａｇｖ会撞墙。
    int allowValidCount=10;
    static int effectiveCount=0;
    static VehiclePosition posFeedback_old;
    static double deltaDistanceOut_x_old=0;
    static double deltaDistanceOut_y_old=0;
    static double deltaAngleOut_old=0;
    double allowD=0.00001;
    double allowA=0.00001;
    double judgeTrans=0.015;//m/s
    double judgeRotate=0.022;//rad/s . 3deg/s=3.14*3/180=0.052

    if(fabs(posFeedback_old.rotate-posFeedback.rotate)<allowA
            && fabs(posFeedback_old.x-posFeedback.x)<allowD
            && fabs(posFeedback_old.y-posFeedback.y)<allowD)
    {
        //速度大于设定值.不加这个限制会导致低速时不运行动作
        if(fabs(vehicleVelocity.x_move)>judgeTrans
                || fabs(vehicleVelocity.y_move)>judgeTrans
                || fabs(vehicleVelocity.z_rotate)>judgeRotate)
        {
             effectiveCount++;
        }
        else
        {
            effectiveCount=0;
        }
        if(effectiveCount<allowValidCount && effectiveCount>0)
        {
            deltaDistanceOut_x=deltaDistanceOut_x_old;
            deltaDistanceOut_y=deltaDistanceOut_y_old;
            deltaAngleOut=deltaAngleOut_old;
            qDebug()<<"*****************检测到反馈位置连续不变VirtualLineBias::checkPositionFeedbackForBias";
        }
    }
    posFeedback_old=posFeedback;
    deltaDistanceOut_x_old=deltaDistanceOut_x;
    deltaDistanceOut_y_old=deltaDistanceOut_y;
    deltaAngleOut_old=deltaAngleOut;
    return 1;

}

int VirtualLineBias::checkStartMoveOrientation(bool isMoveForward,double carOrientation,double idealMovingAngle)
{
    double enableMaxAngleOffset=1.0;//rad
    double tmpOffset;
    static int errorCount=0;//大范围往复运动，可能误判.旋转后旧位置可能是错的。
    if(isMoveForward)
    {
        tmpOffset=angleRotationJudgement(carOrientation,idealMovingAngle);
        if(fabs(tmpOffset)>enableMaxAngleOffset)
        {
            errorCount++;
            qDebug()<<"checkStartMoveOrientation isMoveForward"<<isMoveForward
                   <<"carOrientation"<<carOrientation<<"idealMovingAngle"<<idealMovingAngle
                  <<"tmpOffset"<<tmpOffset;
        }
        else
        {
            errorCount=0;
            return 1;
        }
    }
    else
    {
        tmpOffset=angleRotationJudgement(transformToPi(carOrientation+M_PI),idealMovingAngle);
        if(fabs(tmpOffset)>enableMaxAngleOffset)
        {
            errorCount++;
            qDebug()<<"checkStartMoveOrientation isMoveForward"<<isMoveForward
                   <<"carOrientation"<<carOrientation<<"idealMovingAngle"<<idealMovingAngle
                  <<"tmpOffset"<<tmpOffset;
        }
        else
        {
            errorCount=0;
            return 1;
        }
    }

    if(errorCount>3)
    {
        return -1;
    }
    return 1;
}


//int VirtualLineBias::calculateAbsoluteBias_end2(double &absoluteDistanceOut, double &absoluteAngleOut)
//{

//   VehiclePosition targetPos= coordinateTrajectory->targetPosition;
//   VehiclePosition feedbackPos= coordinateTrajectory->vehiclePositionFeedback;

//    double idealMovingAngle;
//    double actMovingAngle;
//    bool isPassOver=false;
//    idealMovingAngle=lastMoveDirection;//(-pi,pi)
////    if(fabs(feedbackPos.y-targetPos.y)<0.03 || fabs(feedbackPos.x-targetPos.x)<0.03)
////    {
////        absoluteDistanceOut=absoluteDistance_old;
////        absoluteAngleOut=absoluteAngle_old;
////        return 1;
////    }
//    actMovingAngle=atan2(feedbackPos.y-targetPos.y,feedbackPos.x-targetPos.x);//(-pi,pi)有误差
//     qDebug()<<"!!!!!!!!!feedbackPos.y"<<feedbackPos.y<<",  feedbackPos.x"<<feedbackPos.x<<
//               "targetPos.y"<<targetPos.y<<", targetPos.x"<<targetPos.x;
////    getVehicleMovingDirection(actMovingAngle);
//    //direction
//    double tmpDirection;
//    if(true==isPositiveDirection)
//    {
//        tmpDirection=1;
//    }
//    else
//    {
//        tmpDirection=-1;
//    }
//    double totalDistance=sqrt((feedbackPos.x-targetPos.x)*(feedbackPos.x-targetPos.x));
//    double movingAngleBias=transformToPi(idealMovingAngle-actMovingAngle);
//    qDebug()<<"------ lastMoveDirection=="<<lastMoveDirection;

//    double threadholdOfDistance=0.03;
//    if(M_PI_2>=movingAngleBias && -M_PI_2<=movingAngleBias)//要求方向与当前运动方向相反
//    {
//        isPassOver=false;
//        absoluteAngleOut=0;
//        absoluteDistanceOut=totalDistance*tmpDirection;//需要判断位置超前还是滞后。改为正＋
//        qDebug()<<"dddddd tmpAngleBias="<<movingAngleBias<<"actAngle"<<actMovingAngle
//               <<"  absoluteDistanceOut"<<absoluteDistanceOut;
//        //限制输出
//        if(true==isPositiveDirection)
//        {
//            if(fabs(absoluteDistanceOut)>threadholdOfDistance)
//            {
//                absoluteDistanceOut=threadholdOfDistance;
//            }
//        }
//        else
//        {

//            if(fabs(absoluteDistanceOut)>threadholdOfDistance)
//            {
//                absoluteDistanceOut=-threadholdOfDistance;
//            }
//        }


//    }
//    else//要求方向与当前运动方向相同
//    {
//        isPassOver=true;
//        absoluteAngleOut=0;
//        absoluteDistanceOut=-totalDistance*tmpDirection;//需要判断位置超前还是滞后。改为负－
//        qDebug()<<"rrrrrr tmpAngleBias="<<movingAngleBias<<"actAngle"<<actMovingAngle
//               <<"  absoluteDistanceOut"<<absoluteDistanceOut;
//        //限制输出
//        if(true==isPositiveDirection)
//        {
//            if(fabs(absoluteDistanceOut)>threadholdOfDistance)
//            {
//                absoluteDistanceOut=-threadholdOfDistance;
//            }
//        }
//        else
//        {

//            if(fabs(absoluteDistanceOut)>threadholdOfDistance)
//            {
//                absoluteDistanceOut=threadholdOfDistance;
//            }
//        }

//    }

////    if(true==isPositiveDirection && absoluteDistanceOut>0)
////    {
////        absoluteDistanceOut=0;
////        qDebug()<<"not allow true==isPositiveDirection && absoluteDistanceOut>0";
////    }
////    else if(false==isPositiveDirection && absoluteDistanceOut<0)
////    {
////        absoluteDistanceOut=0;
////        qDebug()<<"not allow false==isPositiveDirection && absoluteDistanceOut<0";
////    }



//    return 1;


//}

double VirtualLineBias::transformToPi(double angleIn)
{
//    int turns=angleIn/M_PI/2;//可能bug 参考getTurnsFromRad
    int turns;

    if(0<angleIn)
    {
        turns=(angleIn+M_PI) /(2*M_PI);
    }
    else
    {
        turns=(angleIn-M_PI) /(2*M_PI);
    }

    //    angleIn=angleIn-turns*M_PI;//错误
        angleIn=angleIn-turns*M_PI*2;

    if(angleIn>=-M_PI && angleIn<=M_PI)
    {
        return angleIn;
    }
    else if(angleIn<-M_PI)
    {
        return angleIn+2*M_PI;
    }
    else if(angleIn>M_PI)
    {
        return angleIn-2*M_PI;
    }

    return angleIn;
}

int VirtualLineBias::calculateBias(VehiclePosition vehiclePositionCommandOld,
                                   VehiclePosition vehiclePositionCommand,
                                   double absoluteDistanceIn, double absoluteAngleIn,
                                   double &followDistanceError,double &followAngleError)
{

    //todo 直线开头一段防止倒退
    double tmpMovingAngleBias=0;
    absoluteDistance=absoluteDistanceIn;//距离目标点的距离
    absoluteAngle=absoluteAngleIn;//距离目标点的角度
    renewPidPara();
    followDistanceError=0;//给初始值。
    followAngleError=0;//给初始值。
    switch(calculateBiasType)
    {
    case E_BIAS_TYPE_STEERING_WAIT:
    case E_BIAS_TYPE_NONE:
    {
        stopOutput();
        break;
    }
    case E_BIAS_TYPE_IDILE:
    {
        forward_x=0;
        forward_y=0;
        forward_r=0;
        isPidStop=false;
        break;
    }
    case E_BIAS_TYPE_ROTATE:
    {
        int tmpKey;
        double tmp_deltaAngle,tmp_forward_r;
        if(E_PATH_CONTROL_STRATEGY_DIFFERENTIAL==pathControlStrategy)
        {
            tmpKey=calculateMicroBias_rotation(deltaTime,vehiclePositionCommandOld,
                             vehiclePositionCommand, currentVehiclePosition,currentVehiclePosition_old,
                             followDistanceError,followAngleError,tmp_deltaAngle,tmp_forward_r,
                                               tmpMovingAngleBias,lastBiasType);

        }
        else if(E_PATH_CONTROL_STRATEGY_MCCRUM==pathControlStrategy)
        {
            tmpKey=calculateMicroBias_rotation(deltaTime,vehiclePositionCommandOld,
                             vehiclePositionCommand, currentVehiclePosition,currentVehiclePosition_old,
                             followDistanceError,followAngleError,tmp_deltaAngle,tmp_forward_r,
                                               tmpMovingAngleBias,lastBiasType);

        }
        forward_x=0;
        forward_y=0;
        if(lastBiasType_old!=E_BIAS_TYPE_ROTATE)//多剔除一个点
        {
            if(D_PRINT_VIRTUAL_MOVING_DEBUGTTT7 )
            {
               qDebug()<<"debug7: lastBiasType_old!=E_BIAS_TYPE_ROTATE";
            }

            tmp_deltaAngle=0;
            tmp_forward_r=0;
            followDistanceError=0;
            followAngleError=0;

        }
        deltaDistance_x=0;
        deltaDistance_y=0;
        forward_x=0;
        forward_y=0;//防止原地旋转时，有其它分量的输出。
        deltaAngle=tmp_deltaAngle;
        forward_r=tmp_forward_r;
//        qDebug()<<"E_BIAS_TYPE_ROTATE forward_r"<<forward_r;

//        if(1!=tmpKey)
//        {
//            return -1;
//        }
//        calculateAbsoluteBias_moving(absoluteDistanceOut,absoluteAngleOut);
        isPidStop=false;
        //有时候几个到几十个周期反馈的位置不变，如果检查到这种情况，则使偏差输出不变，避免一直增大。是因为激光tf没有更新。
//        checkPositionFeedbackForBias(currentVehiclePosition,currentVehicleVelocity_feedback,
//                                     deltaDistance_x,deltaDistance_y,deltaAngle);
        break;
    }
    case E_BIAS_TYPE_MOVE_LINE:
    {
        int tmpKey;
        double tmp_deltaAngle,tmp_forward_x,tmp_forward_y,tmp_forward_r,tmp_deltaDistance_x,tmp_deltaDistance_y;
        tmp_forward_x=0;tmp_forward_y=0;tmp_forward_r=0;
        if(E_PATH_CONTROL_STRATEGY_DIFFERENTIAL==pathControlStrategy)
        {
            tmp_deltaDistance_y=0;//差分策略，必须归零．
            tmpKey=calculateMicroBias_stanley(deltaTime,vehiclePositionCommandOld,vehiclePositionCommand,
                                              currentVehiclePosition,currentVehiclePosition_old,
                                              followDistanceError,followAngleError,
                                              tmp_deltaDistance_x,tmp_deltaAngle,tmp_forward_x,tmp_forward_r,
                                              tmpMovingAngleBias,lastBiasType);
            forward_y=0;
            if(1==tmpKey)
            {
                //tmp_deltaAngle=filtering_moveR->arithmeticMeanfilter(tmp_deltaAngle,vmark_filterCount);
                tmp_deltaAngle=filtering_moveR->medianMean(tmp_deltaAngle,vmark_filterCount);//效果不好．
                // tmp_deltaAngle=filtering_moveR->amplitudeLimiterFilter(tmp_deltaAngle,0.068);
            }
            else
            {
                filtering_moveR->resetAll();
            }



        }
        else if(E_PATH_CONTROL_STRATEGY_MCCRUM==pathControlStrategy)
        {
            tmpKey=calculateMicroBias_maccrum(deltaTime,vehiclePositionCommandOld,vehiclePositionCommand,
                                              currentVehiclePosition,currentVehiclePosition_old,
                                              followDistanceError,followAngleError,
                                              tmp_deltaDistance_x,tmp_deltaDistance_y,tmp_deltaAngle,
                                              tmp_forward_x,tmp_forward_y,tmp_forward_r,
                                              tmpMovingAngleBias,lastBiasType);
//            if(false==isEnableKpChange)
//            {
//                tmp_deltaDistance_y=filtering_moveY->arithmeticMeanfilter(tmp_deltaDistance_y,vmark_filterCount);
     /*           tmp_deltaDistance_y=filtering_moveY->medianMean(tmp_deltaDistance_y,vmark_filterCount);//
                tmp_deltaDistance_x=filtering_moveR->medianMean(tmp_deltaDistance_x,vmark_filterCount);*///
//            }
//            tmp_deltaDistance_x=filtering_moveX->medianMean(tmp_deltaDistance_x,common_filterCount);
        }
        if(lastBiasType_old!=E_BIAS_TYPE_MOVE_LINE)//多剔除一个点
        {
            if(D_PRINT_VIRTUAL_MOVING_DEBUGTTT8 )
            {
                qDebug()<<"debug8:lastBiasType_old!=E_BIAS_TYPE_MOVE_LINE";
            }

            tmp_deltaAngle=0;
            tmp_forward_r=0;
            tmp_deltaDistance_x=0;
            tmp_deltaDistance_y=0;
            followDistanceError=0;
            followAngleError=0;
            filtering_moveX->resetAll();
            filtering_moveY->resetAll();
            filtering_moveR->resetAll();

//            qDebug()<<"lastBiasType_old!=E_BIAS_TYPE_MOVE_LINE";

        }
        if(0>tmpKey)//-99>tmpKey
        {
            return tmpKey;
        }
        deltaAngle=tmp_deltaAngle;
//        qDebug()<<"1tmp_forward_r"<<tmp_forward_r;
        //forward_r=tmp_forward_r;圆弧时前馈导致速度突变。
        //前馈会导致减速停止时前后震荡
//        forward_x=filtering_feedforwardX->arithmeticMeanfilter(tmp_forward_x,D_FORWARD_R_FILTER_COUNT);
//        forward_y=filtering_feedforwardY->arithmeticMeanfilter(tmp_forward_y,D_FORWARD_R_FILTER_COUNT);
//        forward_r=filtering_feedforwardR->arithmeticMeanfilter(tmp_forward_r,D_FORWARD_R_FILTER_COUNT);
        forward_x=tmp_forward_x;
        forward_y=tmp_forward_y;
        forward_r=tmp_forward_r;
        deltaDistance_x=tmp_deltaDistance_x;
        deltaDistance_y=tmp_deltaDistance_y;
        followAngleError=tmp_deltaAngle;
//         qDebug()<<"E_BIAS_TYPE_MOVE_LINE forward_x"<<forward_x<<"forward_r"<<forward_r;

//        int tmpKey=calculateMicroBias(coordinateTrajectory->vehiclePositionCommandOld,
//                                      coordinateTrajectory->vehiclePositionCommand,
//                           currentVehiclePosition,deltaDistanceOut,deltaAngleOut);

//        calculateAbsoluteBias_moving(absoluteDistanceOut,absoluteAngleOut);
        isPidStop=false;
        //有时候几个到几十个周期反馈的位置不变，如果检查到这种情况，则使偏差输出不变，避免一直增大。是因为激光tf没有更新。
        //如果取消该函数，会导致vmark模式速度过快。
//        checkPositionFeedbackForBias(currentVehiclePosition,currentVehicleVelocity_feedback,
//                                     deltaDistance_x,deltaDistance_y,deltaAngle);


        break;
    }
    case E_BIAS_TYPE_ABSOLUTE://轨迹运算结束时,效果不好，不要等了
    {
//        calculateAbsoluteBias_end2(absoluteDistanceOut,absoluteAngleOut);
//        calculateAbsoluteBias_moving(absoluteDistanceOut,absoluteAngleOut);
//        deltaDistanceOut=absoluteDistanceOut;
//        deltaAngleOut=absoluteAngleOut;
//        isPidStop=false;
        deltaDistance_x=0;
        deltaAngle=0;
        isPidStop=true;
        break;

    }
    }



//    deltaDistanceOut=deltaDistance_x;
//    deltaAngleOut=deltaAngle;
    double tmpForward=forward_x;
    if(2==debugFlag)
    {//加forward_x打印时，数据被改变了？
        qDebug()<<"deltaDistance_x"<<deltaDistance_x<<" calculateBiasType"<<calculateBiasType<<"forward_x"<<tmpForward;
    }

//    qDebug()<<"为　deltaDistance_x"<<deltaDistance_x<<" calculateBiasType"<<calculateBiasType;

    lastBiasType_old=lastBiasType;
    lastBiasType=calculateBiasType;



    if(deltaAngle>8 || forward_r>8)
    {
        qDebug()<<"error deltaAngle"<<deltaAngle<<forward_r;//
    }

//   qDebug()<<"33333a3 rotateFeedwardOut"<<forward_r;
//    qDebug()<<"followDistanceError"<<followDistanceError<<"followAngleError"<<followAngleError;
    return 1;

}

int VirtualLineBias::setActualFollowOffset(double distanceOffsetIn,double angleOffsetIn)
{
    distanceFollowError=distanceOffsetIn;
    angleFollowError=angleOffsetIn;
    return 1;
}

//int VirtualLineBias::calculateAbsoluteBias_moving(double &absoluteDistanceOut, double &absoluteAngleOut)
//{
//    return coordinateTrajectory->getAbsoluteVehiclePositionLeft(absoluteDistanceOut,absoluteAngleOut);
//}

//int VirtualLineBias::calculateAbsoluteBias_end(double &absoluteDistanceOut, double &absoluteAngleOut)
//{

//   VehiclePosition targetPos= coordinateTrajectory->targetPosition;
//   VehiclePosition feedbackPos= coordinateTrajectory->vehiclePositionFeedback;

//    double idealMovingAngle;
//    double actMovingAngle;
//    bool isPassOver=false;
//    idealMovingAngle=lastMoveDirection;//(-pi,pi)
////    if(fabs(feedbackPos.y-targetPos.y)<0.03 || fabs(feedbackPos.x-targetPos.x)<0.03)
////    {
////        absoluteDistanceOut=absoluteDistance_old;
////        absoluteAngleOut=absoluteAngle_old;
////        return 1;
////    }
//    actMovingAngle=atan2(feedbackPos.y-targetPos.y,feedbackPos.x-targetPos.x);//(-pi,pi)有误差
//     qDebug()<<"!!!!!!!!!feedbackPos.y"<<feedbackPos.y<<",  feedbackPos.x"<<feedbackPos.x<<
//               "targetPos.y"<<targetPos.y<<", targetPos.x"<<targetPos.x;
////    getVehicleMovingDirection(actMovingAngle);
//    //direction
//    double tmpDirection;
//    if(true==isPositiveDirection)
//    {
//        tmpDirection=1;
//    }
//    else
//    {
//        tmpDirection=-1;
//    }
//    double totalDistance=sqrt((feedbackPos.x-targetPos.x)*(feedbackPos.x-targetPos.x));
//    double movingAngleBias=transformToPi(idealMovingAngle-actMovingAngle);
//    qDebug()<<"------ lastMoveDirection=="<<lastMoveDirection;


//    if(M_PI_2>=movingAngleBias && -M_PI_2<=movingAngleBias)
//    {
//        isPassOver=false;
//        absoluteAngleOut=0;
//        absoluteDistanceOut=-totalDistance*tmpDirection;//需要判断位置超前还是滞后。
//        qDebug()<<"dddddd tmpAngleBias="<<movingAngleBias<<"actAngle"<<actMovingAngle
//               <<"  absoluteDistanceOut"<<absoluteDistanceOut;


//    }
//    else
//    {
//        isPassOver=true;
//        absoluteAngleOut=0;
//        absoluteDistanceOut=totalDistance*tmpDirection;//需要判断位置超前还是滞后。
//        qDebug()<<"rrrrrr tmpAngleBias="<<movingAngleBias<<"actAngle"<<actMovingAngle
//               <<"  absoluteDistanceOut"<<absoluteDistanceOut;
//    }

////    if(true==isPositiveDirection && absoluteDistanceOut>0)
////    {
////        absoluteDistanceOut=0;
////        qDebug()<<"not allow true==isPositiveDirection && absoluteDistanceOut>0";
////    }
////    else if(false==isPositiveDirection && absoluteDistanceOut<0)
////    {
////        absoluteDistanceOut=0;
////        qDebug()<<"not allow false==isPositiveDirection && absoluteDistanceOut<0";
////    }



//    return 1;


//}

int VirtualLineBias::calculateTargetBias(VehiclePosition targetPos,double &deltaDistanceOut, double &deltaAngleOut)
{
    //direction
    double tmpDirection;
    if(true==isPositiveDirection)
    {
        tmpDirection=1;
    }
    else
    {
        tmpDirection=-1;
    }

    deltaDistanceOut=-sqrt((targetPos.x-currentVehiclePosition.x)*(targetPos.x-currentVehiclePosition.x))*tmpDirection;

//        deltaAngleOut=-tmpAngleBias;
//        deltaDistanceOut=-sqrt((targetPos.y-currentVehiclePosition.y)*(targetPos.y-currentVehiclePosition.y)+
//                              (targetPos.x-currentVehiclePosition.x)*(targetPos.x-currentVehiclePosition.x))*cos(deltaAngleOut)*tmpDirection;

    return 1;
}
